
#include <cstring>
#include<cassert>
#include <vector>
#include <iostream>
#include <cmath>

#include "Vector3D.h"


// Constructors
Vector3D::Vector3D() {
    mCoords[0] = 0;
    mCoords[1] = 0;
    mCoords[2] = 0;
}

Vector3D::Vector3D(double x) {
    mCoords[0] = x;
    mCoords[1] = x;
    mCoords[2] = x;
}

Vector3D::Vector3D(double x, double y, double z) {
    mCoords[0] = x;
    mCoords[1] = y;
    mCoords[2] = z;
}

Vector3D::Vector3D(const double xyz[3]) {
    std::memcpy(mCoords, xyz, 3 * sizeof(double));
}

Vector3D::Vector3D(const Vector3D& to_copy) {
    std::memcpy(mCoords, to_copy.mCoords, 3 * sizeof(double));
}

// Functions to get coordinates
double Vector3D::x() const { return mCoords[0]; }

double Vector3D::y() const { return mCoords[1]; }

double Vector3D::z() const { return mCoords[2]; }

void Vector3D::get_coordinates(double& x, double& y, double& z) const {
    x = mCoords[0];
    y = mCoords[1];
    z = mCoords[2];
}

void Vector3D::get_coordinates(double xyz[3]) const {
    std::memcpy(xyz, mCoords, 3 * sizeof(double));
}

const double& Vector3D::operator[](size_t index) const {
    return mCoords[index];
}

// Functions to set coordinates
void Vector3D::x(const double x) { mCoords[0] = x; }

void Vector3D::y(const double y) { mCoords[1] = y; }

void Vector3D::z(const double z) { mCoords[2] = z; }

void Vector3D::set(const double x, const double y, const double z) {
    mCoords[0] = x;
    mCoords[1] = y;
    mCoords[2] = z;
}

void Vector3D::set(const double xyz[3]) {
    std::memcpy(mCoords, xyz, 3 * sizeof(double));
}

void Vector3D::set(const Vector3D& to_copy) {
    std::memcpy(mCoords, to_copy.mCoords, 3 * sizeof(double));
}

double& Vector3D::operator[](size_t index) { return mCoords[index]; }

Vector3D& Vector3D::operator=(const Vector3D& to_copy) {
    mCoords[0] = to_copy.mCoords[0];
    mCoords[1] = to_copy.mCoords[1];
    mCoords[2] = to_copy.mCoords[2];
    //    memcpy(mCoords, to_copy.mCoords, 3*sizeof(double));
    return *this;
}

Vector3D& Vector3D::operator=(const double& to_copy) {
    mCoords[0] = to_copy;
    mCoords[1] = to_copy;
    mCoords[2] = to_copy;
    return *this;
}

// Functions that modify existing coordinates
Vector3D Vector3D::operator-() const {
    return Vector3D(-mCoords[0], -mCoords[1], -mCoords[2]);
}

Vector3D& Vector3D::operator*=(const double scalar) {
    mCoords[0] *= scalar;
    mCoords[1] *= scalar;
    mCoords[2] *= scalar;
    return *this;
}

//! divides each Vector3D entry by the given scalar.
Vector3D& Vector3D::operator/=(const double scalar) {
    mCoords[0] /= scalar;
    mCoords[1] /= scalar;
    mCoords[2] /= scalar;
    return *this;
}

Vector3D& Vector3D::operator*=(const Vector3D& rhs) {
    double new_coords[3] = {
            mCoords[1] * rhs.mCoords[2] - mCoords[2] * rhs.mCoords[1],
            mCoords[2] * rhs.mCoords[0] - mCoords[0] * rhs.mCoords[2],
            mCoords[0] * rhs.mCoords[1] - mCoords[1] * rhs.mCoords[0] };
    std::memcpy(mCoords, new_coords, 3 * sizeof(double));
    return *this;
}

Vector3D& Vector3D::operator+=(const Vector3D& rhs) {
    mCoords[0] += rhs.mCoords[0];
    mCoords[1] += rhs.mCoords[1];
    mCoords[2] += rhs.mCoords[2];
    return *this;
}

Vector3D& Vector3D::operator-=(const Vector3D& rhs) {
    mCoords[0] -= rhs.mCoords[0];
    mCoords[1] -= rhs.mCoords[1];
    mCoords[2] -= rhs.mCoords[2];
    return *this;
}

// Binary operators
const Vector3D operator+(const Vector3D& lhs, const Vector3D& rhs) {
    return Vector3D(lhs.x() + rhs.x(), lhs.y() + rhs.y(), lhs.z() + rhs.z());
}

const Vector3D operator-(const Vector3D& lhs, const Vector3D& rhs) {
    return Vector3D(lhs.x() - rhs.x(), lhs.y() - rhs.y(), lhs.z() - rhs.z());
}

const Vector3D operator*(const Vector3D& lhs, const double scalar) {
    return Vector3D(lhs.x() * scalar, lhs.y() * scalar, lhs.z() * scalar);
}

const Vector3D operator*(const double scalar, const Vector3D& rhs) {
    return Vector3D(rhs.x() * scalar, rhs.y() * scalar, rhs.z() * scalar);
}

const Vector3D operator/(const Vector3D& lhs, const double scalar) {
    assert(scalar != 0);
    return Vector3D(lhs.x() / scalar, lhs.y() / scalar, lhs.z() / scalar);
}

double operator%(const Vector3D& lhs,
    const Vector3D& rhs) // Dot Product
{
    return (lhs.mCoords[0] * rhs.mCoords[0] + lhs.mCoords[1] * rhs.mCoords[1] +
        lhs.mCoords[2] * rhs.mCoords[2]);
}

/*! Dot product for arrays of Vector3Ds. see also operator% .*/
double inner(const Vector3D lhs[], const Vector3D rhs[], int n) {
    int i;
    double dot = 0;
    for (i = 0; i < n; ++i)
        dot += lhs[i].mCoords[0] * rhs[i].mCoords[0] +
        lhs[i].mCoords[1] * rhs[i].mCoords[1] +
        lhs[i].mCoords[2] * rhs[i].mCoords[2];
    return dot;
}

/*! Dot product for arrays of Vector3Ds. see also operator% .*/
double inner(const std::vector<Vector3D>& lhs,
    const std::vector<Vector3D>& rhs) {
    double dot = 0;
    assert(lhs.size() == rhs.size());
    for (size_t i = 0; i < lhs.size(); ++i) dot = lhs[i] % rhs[i];
    return dot;
}

double operator%(const double scalar,
    const Vector3D& rhs) // Dot Product
{
    return (scalar * (rhs.mCoords[0] + rhs.mCoords[1] + rhs.mCoords[2]));
}

double operator%(const Vector3D& lhs,
    const double scalar) // Dot Product
{
    return (scalar * (lhs.mCoords[0] + lhs.mCoords[1] + lhs.mCoords[2]));
}

const Vector3D operator*(const Vector3D& lhs,
    const Vector3D& rhs) // Cross Product
{
    return Vector3D(
        lhs.mCoords[1] * rhs.mCoords[2] - lhs.mCoords[2] * rhs.mCoords[1],
        lhs.mCoords[2] * rhs.mCoords[0] - lhs.mCoords[0] * rhs.mCoords[2],
        lhs.mCoords[0] * rhs.mCoords[1] - lhs.mCoords[1] * rhs.mCoords[0]);
}

// output operator
std::ostream& operator<<(std::ostream& s, const Vector3D& v);

double Vector3D::distance_between(const Vector3D& p1, const Vector3D& p2) {
    Vector3D v = p2 - p1;
    return v.length();
}

int Vector3D::within_tolerance_box(const Vector3D& compare_to,
    double tolerance) const {
    return ((fabs(this->mCoords[0] - compare_to.mCoords[0]) < tolerance) &&
        (fabs(this->mCoords[1] - compare_to.mCoords[1]) < tolerance) &&
        (fabs(this->mCoords[2] - compare_to.mCoords[2]) < tolerance));
}

// Length functions
double Vector3D::length_squared() const {
    return (mCoords[0] * mCoords[0] + mCoords[1] * mCoords[1] +
        mCoords[2] * mCoords[2]);
}

double Vector3D::length() const {
    return sqrt(mCoords[0] * mCoords[0] + mCoords[1] * mCoords[1] +
        mCoords[2] * mCoords[2]);
}

double inner_product(const Vector3D* v1, const Vector3D* v2, size_t n) {
    double result = 0.0;
    const Vector3D* const end = v1 + n;
    while (v1 < end) {
        result += *v1 % *v2;
        ++v1;
        ++v2;
    }
    return result;
}

double length_squared(const Vector3D* v, int n) {
    double sum = 0.0;
    for (int i = 0; i < n; ++i) sum += v[i].length_squared();
    return sum;
}

double length_squared(const std::vector<Vector3D>& v) {
    double sum = 0.0;
    for (size_t i = 0; i < v.size(); ++i) sum += v[i].length_squared();
    return sum;
}

double length(const Vector3D* v, int n) // norm for an array of Vector3Ds
{
    return sqrt(length_squared(v, n));
}

double length(const std::vector<Vector3D>& v) {
    return sqrt(length_squared(v));
}

double Linf(const Vector3D* v,
    int n) // max entry for an array of Vector3Ds
{
    double max = 0;
    // loop over the length of the array
    for (int i = 0; i < n; ++i) {
        if (max < fabs(v[i][0])) max = fabs(v[i][0]);
        if (max < fabs(v[i][1])) max = fabs(v[i][1]);
        if (max < fabs(v[i][2])) max = fabs(v[i][2]);
    }
    // return the value_ of the largest entry in the array
    return max;
}

double Linf(
    const std::vector<Vector3D>& v) // max entry for an array of Vector3Ds
{
    double max = 0;
    // loop over the length of the array
    for (size_t i = 0; i < v.size(); ++i) {
        if (max < fabs(v[i][0])) max = fabs(v[i][0]);
        if (max < fabs(v[i][1])) max = fabs(v[i][1]);
        if (max < fabs(v[i][2])) max = fabs(v[i][2]);
    }
    // return the value_ of the largest entry in the array
    return max;
}

void Vector3D::set_length(const double new_length) {
    double factor = new_length / length();
    *this *= factor;
}

void Vector3D::normalize() { set_length(1.0); }

// Utility functions.
Vector3D Vector3D::interpolate(const double param, const Vector3D& p1,
    const Vector3D& p2) {
    return (1 - param) * p1 + param * p2;
}

bool operator==(const Vector3D& v1, const Vector3D& v2) {
    return v1.mCoords[0] == v2.mCoords[0] && v1.mCoords[1] == v2.mCoords[1] &&
        v1.mCoords[2] == v2.mCoords[2];
}

bool operator!=(const Vector3D& v1, const Vector3D& v2) {
    return v1.mCoords[0] != v2.mCoords[0] || v1.mCoords[1] != v2.mCoords[1] ||
        v1.mCoords[2] != v2.mCoords[2];
}